import rospy
import serial
import struct
from sensor_msgs.msg import JointState

class MasterArmController:
    def __init__(self):
        rospy.init_node('puppet_arm_controller', anonymous=True)
        
        # 可配置参数
        self.joint_count = 6  # 期望的关节数量
        self.serial_port = rospy.get_param('~port', '/dev/ttyACM1')
        self.baudrate = rospy.get_param('~baudrate', 115200)
        
        try:
            self.ser = serial.Serial(
                port=self.serial_port,
                baudrate=self.baudrate,
                timeout=1,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE
            )
            rospy.loginfo("Serial port opened successfully")
        except serial.SerialException as e:
            rospy.logerr("Failed to open serial port: %s", e)
            rospy.signal_shutdown("Serial port not opened")
            return
        
        # 初始化关节位置，从 1 开始
        self.joint_positions = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]

        # 设置定时器，每秒发送一次数据
        self.timer = rospy.Timer(rospy.Duration(1.0), self.send_data)
        rospy.loginfo("Node initialized and ready")

    def send_data(self, event):
        # 每次发送数据时，每个关节值 +1
        self.joint_positions = [x + 1 for x in self.joint_positions]
        rospy.loginfo("Updated joint positions: %s", self.joint_positions)
        
        try:
            # 使用小端字节序打包数据
            packed_data = struct.pack('<%df' % self.joint_count, *self.joint_positions)
            # 创建数据头 0xA5
            header = bytearray([0xA5])
            # 拼接数据头和打包的数据
            final_data = header + packed_data
            self.ser.write(final_data)
            rospy.loginfo("Sent packed data: %s", final_data)
        except Exception as e:
            rospy.logerr("Error sending data to serial port: %s", e)
            self.ser.close()
            rospy.signal_shutdown("Serial communication error")

if __name__ == '__main__':
    try:
        controller = MasterArmController()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    finally:
        if hasattr(controller, 'ser') and controller.ser.isOpen():
            controller.ser.close()
            rospy.loginfo("Serial port closed")
